#include "cuboid.h"
#include <cmath>

//给出机器人的朝向，并设置它的旋转轴，根据题意，z轴始终沿z轴方向，不需要改变
void Cuboid::setAxe(const float& rotation) {
    m_axis[0].m_x = m_axis[1].m_y = cos(rotation);
    m_axis[0].m_y = sin(rotation);
    m_axis[1].m_x = -sin(rotation);
}
void Cuboid::setVert(const float& rotation){
    DR.m_x = m_halflength*cos(rotation) + m_halfwidth*sin(rotation) + m_center.m_x;
    DR.m_y = m_halflength*sin(rotation) - m_halfwidth*cos(rotation) + m_center.m_y;
    UR.m_x = m_halflength*cos(rotation) - m_halfwidth*sin(rotation) + m_center.m_x;
    UR.m_y = m_halflength*sin(rotation) + m_halfwidth*cos(rotation) + m_center.m_y;
    UL.m_x = -m_halflength*cos(rotation) - m_halfwidth*sin(rotation)+ m_center.m_x;
    UL.m_y = -m_halflength*sin(rotation) + m_halfwidth*cos(rotation) + m_center.m_y;
    DL.m_x = -m_halflength*cos(rotation) + m_halfwidth*sin(rotation) + m_center.m_x;
    DL.m_y = -m_halflength*sin(rotation) - m_halfwidth*cos(rotation) + m_center.m_y;
}

float Cuboid::operator[](int i) {
    float b[3] = { m_halflength,m_halfwidth,m_halfheight };
    return b[i];
}
float Cuboid::operator[](int i) const{
    float b[3] = { m_halflength,m_halfwidth,m_halfheight };
    return b[i];
}
